//
// Created by lqk on 2022/1/30.
//

#ifndef QUADCEREBELLUM_DEFAULTSWINGLEGPLANNER_H
#define QUADCEREBELLUM_DEFAULTSWINGLEGPLANNER_H

#include "Eigen/Core"
class CubicBezier{
public:
    Eigen::Vector3f CubicBezierZeroOrder(Eigen::Vector3f p0,Eigen::Vector3f pf,float phase);
    Eigen::Vector3f CubicBezierFirstOrder(Eigen::Vector3f p0,Eigen::Vector3f pf,float phase);
    Eigen::Vector3f CubicBezierSecondOrder(Eigen::Vector3f p0,Eigen::Vector3f pf,float phase);

    float CubicBezierZeroOrder(float,float,float);
    float CubicBezierFirstOrder(float,float,float);
    float CubicBezierSecondOrder(float,float,float);
};

enum class CurveType{
    cubicBezier,
    bSpline,
    cycloid
};

class DefaultSwingLegPlanner {
public:
    DefaultSwingLegPlanner():m_curveType(CurveType::cubicBezier){}
    DefaultSwingLegPlanner(CurveType curveType): m_curveType(curveType){}
    void SetInitialPosition(Eigen::Vector3f p0){m_pInitial=p0;}
    void SetFinalPosition(Eigen::Vector3f pf){m_pFinal=pf;}
    void SetHeight(float h){m_height=h;}
    void SetSwingPeriod(float p){m_swingPeriod=p;}
    Eigen::Vector3f GetPosition(){return m_pInstant;}
    Eigen::Vector3f GetVelocity(){return m_vInstant;}
    Eigen::Vector3f GetAcceleration(){return m_aInstant;}
    void Compute(float phase);
private:
    Eigen::Vector3f m_pInitial,m_pFinal;
    Eigen::Vector3f m_pInstant,m_vInstant,m_aInstant;
    float m_height,m_swingPeriod;
    CurveType m_curveType;
    CubicBezier m_cubicBezier;

};


#endif //QUADCEREBELLUM_DEFAULTSWINGLEGPLANNER_H
